#include  "zos.h"
#include "zos_config.h"
#ifdef BOARD_USING_I2C_DEMO


//****************************************
// 定义MPU6050内部地址
//****************************************
#define	SMPLRT_DIV		0x19	//陀螺仪采样率，典型值：0x07(125Hz)
#define	CONFIG			0x1A	//低通滤波频率，典型值：0x06(5Hz)
#define	GYRO_CONFIG		0x1B	//陀螺仪自检及测量范围，典型值：0x18(不自检，2000deg/s)
#define	ACCEL_CONFIG	0x1C	//加速计自检、测量范围及高通滤波频率，典型值：0x01(不自检，2G，5Hz)
#define	ACCEL_XOUT_H	0x3B
#define	ACCEL_XOUT_L	0x3C
#define	ACCEL_YOUT_H	0x3D
#define	ACCEL_YOUT_L	0x3E
#define	ACCEL_ZOUT_H	0x3F
#define	ACCEL_ZOUT_L	0x40
#define	TEMP_OUT_H		0x41
#define	TEMP_OUT_L		0x42
#define	GYRO_XOUT_H		0x43
#define	GYRO_XOUT_L		0x44	
#define	GYRO_YOUT_H		0x45
#define	GYRO_YOUT_L		0x46
#define	GYRO_ZOUT_H		0x47
#define	GYRO_ZOUT_L		0x48
#define	PWR_MGMT_1		0x6B	//电源管理，典型值：0x00(正常启用)
#define	WHO_AM_I		0x75	//IIC地址寄存器(默认数值0x68，只读)
#define	SlaveAddress	0x68	//IIC地址

//任务全局变量
zos_task_t  mpu6050_i2c_task_handle = NULL;

static void mpu6050_init(void)
{
	zos_uint8_t txdata[2] = {0};
	zos_uint8_t ret = ZOS_EOK;
	zos_uint16_t devaddress = 0x68;
	txdata[0] = 0x6B;
	txdata[1] = 0x80;
	ret = zos_i2c_write(ZOS_I2C_BUS_0,devaddress,txdata,2);
	if(ret != ZOS_EOK) xy_assert(0);
	txdata[0] = SMPLRT_DIV;
	txdata[1] = 0x07;
	ret =zos_i2c_write(ZOS_I2C_BUS_0,SlaveAddress,txdata,2);
	if(ret != ZOS_EOK) xy_assert(0);
	txdata[0] = CONFIG;
	txdata[1] = 0x06;
	ret = zos_i2c_write(ZOS_I2C_BUS_0,SlaveAddress,txdata,2);
	if(ret != ZOS_EOK) xy_assert(0);
	txdata[0] = GYRO_CONFIG;
	txdata[1] = 0x18;
	ret = zos_i2c_write(ZOS_I2C_BUS_0,SlaveAddress,txdata,2);
	if(ret != ZOS_EOK) xy_assert(0);
	txdata[0] = ACCEL_CONFIG;
	txdata[1] = 0x01;
	ret = zos_i2c_write(ZOS_I2C_BUS_0,SlaveAddress,txdata,2);
	if(ret != ZOS_EOK) xy_assert(0);
}

static void get_mpu_data(zos_uint8_t reg, zos_uint8_t *rxdata)
{
	zos_uint8_t ret = ZOS_EOK;
	zos_uint8_t *temp = NULL;
	temp = zos_malloc(sizeof(char)*2);
	temp = rxdata;
	ret = zos_i2c_write(ZOS_I2C_BUS_0,SlaveAddress,&reg,1);
	if(ret != ZOS_EOK) xy_assert(0);
	ret = zos_i2c_read(ZOS_I2C_BUS_0,SlaveAddress,temp,2);
	zos_printf(" is %d   %d\r\n",temp[0],temp[1]);
	if(ret != ZOS_EOK) xy_assert(0);

	zos_free(temp);
	temp = NULL;
}

/**
 * @brief 任务线程
 */
void mpu6050_iic(void)
{
	zos_uint8_t rxdata=0;
	zos_uint8_t txdata[2] = {0};
	ZOS_I2C_CONFIG i2c_cfg;
	zos_uint8_t mode,reg=0;
	i2c_cfg.mode=ZOS_I2C_MODE_MASTER;
	i2c_cfg.speed=ZOS_I2C_SPEED_400K;
	mode=zos_i2c_init(ZOS_I2C_BUS_0,&i2c_cfg);

	if(mode == ZOS_EOK)
	{
		mpu6050_init();
	}
	else
		zos_printf("mpu6050_init ERROR\r\n");


	if(mode == ZOS_EOK)
	{
		reg = ACCEL_XOUT_H;
		zos_i2c_write(ZOS_I2C_BUS_0,SlaveAddress,&reg,1);
		zos_i2c_read(ZOS_I2C_BUS_0,SlaveAddress,&rxdata,1);
		zos_printf("ACCEL_XOUT_H is %d\r\n",rxdata);
		reg = ACCEL_XOUT_L;
		zos_i2c_write(ZOS_I2C_BUS_0,SlaveAddress,&reg,1);
		zos_i2c_read(ZOS_I2C_BUS_0,SlaveAddress,&rxdata,1);
		zos_printf("ACCEL_XOUT_L is %d\r\n",rxdata);
	}
	else
		zos_printf("I2C_INIT ERROR\r\n");

	mpu6050_i2c_task_handle = NULL;
	osThreadExit();
}

void sensor_mpu6050_iic_task(void)
{
	mpu6050_i2c_task_handle = osThreadNew ((osThreadFunc_t)(mpu6050_iic),NULL,"mpu6050_iic",0x400,osPriorityNormal1);
}

#include "shell.h"


#endif //DEMO_TEST
